#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "pico/util/queue.h"
#include "hardware/clocks.h"
#include "hardware/exception.h"

#include "tos_config.h"
#include "cmsis_os.h"

// task1
#define TASK1_STK_SIZE 8192
void task1(void* pdata);
osThreadDef(task1, osPriorityNormal, 1, TASK1_STK_SIZE);

// task2
#define TASK2_STK_SIZE 8192
void task2(void* pdata);
osThreadDef(task2, osPriorityNormal, 1, TASK2_STK_SIZE);

#define TASK3_STK_SIZE 8192
void task3(void* pdata);
osThreadDef(task3, osPriorityNormal, 1, TASK3_STK_SIZE);

#define TASK4_STK_SIZE 8192
void task4(void* pdata);
osThreadDef(task4, osPriorityNormal, 1, TASK4_STK_SIZE);

#define TASK5_STK_SIZE 8192
void task5(void* pdata);
osThreadDef(task5, osPriorityNormal, 1, TASK5_STK_SIZE);

void task1(void* pdata)
{
    int count = 1;

    while (1) {
        printf("1.%i , %d \r\n", cpu_get_index(), count++);
        tos_task_delay(1000);
        // int i = 10000000;
        // while (i--)
        //     __NOP();
        if (count == 2) {
            osThreadCreate(osThread(task4), NULL); // Create task4
            puts("task4 created");
            osThreadCreate(osThread(task5), NULL); // Create task5
            puts("task5 created");
        }

        if (count == 5) {
            puts("task4 terminate...");
            puts(tos_task_destroy(&task_handler_task4) ? "Error!" : "OK!");
        }

        if (count == 10) {
            puts("task5 suspend...");
            puts(tos_task_suspend(&task_handler_task5) ? "ERROR!" : "OK!");
        }

        if (count == 15) {
            puts("task5 resume...");
            puts(tos_task_resume(&task_handler_task5) ? "Error!" : "OK!");
        }

        if (count == 17) {
            puts("task5 suspend...");
            puts(tos_task_suspend(&task_handler_task5) ? "ERROR!" : "OK!");
        }

        if(count == 20){
            puts("task5 delete...");
            puts(tos_task_destroy(&task_handler_task5) ? "Error!" : "OK!");
        }

        if(count == 25){
            puts("task3 delay abort...");
            puts(tos_task_delay_abort(&task_handler_task3) ? "Error!" : "OK!");
        }

        if(count == 28){
            puts("task3 delete...");
            puts(tos_task_destroy(&task_handler_task3) ? "Error!" : "OK!");
        }

        if(count == 33){
            puts("task2 delete...");
            puts(tos_task_destroy(&task_handler_task2) ? "Error!" : "OK!");
        }
    }
}
void task2(void* pdata)
{
    int count = 1;
    while (1) {
        printf("2.%i , %d \r\n", cpu_get_index(), count++);
        tos_task_delay(10000);
        //     int i = 10000000;
        //     while (i--)
        //         __NOP();
    }
}

void task3(void* pdata)
{
    int count = 1;
    while (1) {
        printf("3.%i , %d \r\n", cpu_get_index(), count++);
        osDelay(40000);
        // int i = 10000000;
        // while (i--)
        //     __NOP();
    }
}

void task4(void* pdata)
{
    int count = 1;
    while (1) {
        printf("4.%i , %d \r\n", cpu_get_index(), count++);
        // osDelay(1000);
        int i = 10000000;
        while (i--)
            __NOP();
    }
}

void task5(void* pdata)
{
    int count = 1;
    while (1) {
        printf("5.%i , %d \r\n", cpu_get_index(), count++);
        // osDelay(1000);
        int i = 10000000;
        while (i--)
            __NOP();
    }
}

void SysTick_Handler(void)
{
    if (tos_knl_is_running()) {
        tos_knl_irq_enter();
        tos_tick_handler();
        tos_knl_irq_leave();
    }
}

void core1_entry(void)
{
    printf("Running on Core %d\n", get_core_num());
    multicore_fifo_drain();
    tos_knl_start(); // worker core start
}

extern int application_entry(void* arg);
osThreadDef(application_entry, osPriorityNormal, 1, 8192);

void smp_init()
{
    multicore_fifo_drain();
    spin_lock_claim(PICO_SPINLOCK_ID_OS1);
    spin_lock_claim(PICO_SPINLOCK_ID_OS2);
    spin_lock_init(PICO_SPINLOCK_ID_OS1);
    spin_lock_init(PICO_SPINLOCK_ID_OS2);
}

int main()
{
    stdio_init_all();
    // while (!stdio_usb_connected())
    //     ; // Wait for USB to be connected

    printf("System Clock Frequency is %d Hz\n", clock_get_hz(clk_sys));
    printf("USB Clock Frequency is %d Hz\n", clock_get_hz(clk_usb));
    smp_init();
    osKernelInitialize(); // TOS Tiny kernel initialize
    multicore_launch_core1(core1_entry); // Launch Core1, just leave more time for Core1 to initialize otherwise FIFO could be invalid!
    // osThreadCreate(osThread(task1), NULL); // Create task1
    // osThreadCreate(osThread(task2), NULL); // Create task2
    // osThreadCreate(osThread(task3), NULL); // Create task3
    osThreadCreate(osThread(application_entry), NULL); // Kernel test
    printf("TOS Tiny Kernel Start\n");
    tos_knl_start();

    while (1)
        ;
}
